#include "rob.h"
#include "mfl.h"  /* EASML Math Float Library */

#define PI_F32 3.14159265358979f
#define RAD_TO_DEG_F32 57.2957795131f
#define DEG_TO_RAD_F32 0.0174532925199f

/* Initialize robot configuration and state */
void Rob_Init(Rob_SystemState_Type *State_pst, 
              float32 L0_f32, float32 L1_f32, float32 L2_f32, float32 L3_f32)
{
    uint8 i;
    
    /* Initialize link configuration */
    State_pst->LinkConfig.L0 = L0_f32;
    State_pst->LinkConfig.L1 = L1_f32;
    State_pst->LinkConfig.L2 = L2_f32;
    State_pst->LinkConfig.L3 = L3_f32;
    
    /* Initialize joint angles to zero */
    for (i = 0u; i < ROB_DOF; i++)
    {
        State_pst->CurrentAngles.Theta[i] = 0.0f;
        State_pst->TargetAngles.Theta[i] = 0.0f;
    }
    
    /* Initialize servo control */
    for (i = 0u; i < ROB_SERVO_NUM; i++)
    {
        State_pst->ServoControl.PwmValue[i] = ROB_PWM_CENTER;
        State_pst->ServoControl.CurrentAngle[i] = 0.0f;
        State_pst->ServoControl.TargetAngle[i] = 0.0f;
        State_pst->ServoControl.MoveTime[i] = 0u;
        State_pst->ServoControl.IsMoving[i] = FALSE;
        
        /* Initialize PID controllers using EASML */
        Mfl_PIDSetState(&State_pst->PidStates[i], 0.0f, 0.0f, 0.0f);
        Mfl_PIDSetParam(&State_pst->PidParams[i], 1.0f, 0.1f, 0.01f); /* Default PID gains */
    }
    
    /* Initialize positions */
    State_pst->CurrentPos.X = 0.0f;
    State_pst->CurrentPos.Y = L1_f32 + L2_f32;  /* Straight arm position */
    State_pst->CurrentPos.Z = L0_f32;
    State_pst->CurrentPos.Alpha = 0.0f;
    State_pst->CurrentPos.Beta = 0.0f;
    
    State_pst->TargetPos = State_pst->CurrentPos;
}

/* Inverse kinematics calculation using EASML trigonometric functions */
uint8 Rob_InverseKinematics(float32 X_f32, float32 Y_f32, float32 Z_f32, 
                           float32 Alpha_f32, Rob_SystemState_Type *State_pst)
{
    float32 theta0, theta1, theta2, theta3;
    float32 l0, l1, l2, l3;
    float32 r, z_adj, y_adj;
    float32 cos_theta2, sin_theta2;
    float32 k1, k2;
    float32 alpha_rad;
    
    /* Get link lengths */
    l0 = State_pst->LinkConfig.L0;
    l1 = State_pst->LinkConfig.L1;
    l2 = State_pst->LinkConfig.L2;
    l3 = State_pst->LinkConfig.L3;
    
    /* Convert alpha to radians using EASML */
    alpha_rad = Alpha_f32 * DEG_TO_RAD_F32;
    
    /* Calculate base rotation angle (theta0) */
    if (Mfl_Abs_f32(X_f32) < 0.001f && Mfl_Abs_f32(Y_f32) < 0.001f)
    {
        theta0 = 0.0f;
    }
    else
    {
        theta0 = Mfl_ArcTan2_f32(X_f32, Y_f32) * RAD_TO_DEG_F32;
    }
    
    /* Calculate horizontal distance and adjust for end effector */
    r = Mfl_Sqrt_f32(X_f32 * X_f32 + Y_f32 * Y_f32);
    y_adj = r - l3 * Mfl_Cos_f32(alpha_rad);
    z_adj = Z_f32 - l0 - l3 * Mfl_Sin_f32(alpha_rad);
    
    /* Check workspace limits */
    if (z_adj < -l0)
    {
        return 1u;  /* Below base */
    }
    
    float32 reach = Mfl_Sqrt_f32(y_adj * y_adj + z_adj * z_adj);
    if (reach > (l1 + l2))
    {
        return 2u;  /* Out of reach */
    }
    
    /* Calculate elbow angle (theta2) using EASML cosine law */
    cos_theta2 = (y_adj * y_adj + z_adj * z_adj - l1 * l1 - l2 * l2) / (2.0f * l1 * l2);
    
    /* Check if solution exists */
    if (cos_theta2 > 1.0f || cos_theta2 < -1.0f)
    {
        return 3u;  /* No solution */
    }
    
    sin_theta2 = Mfl_Sqrt_f32(1.0f - cos_theta2 * cos_theta2);
    theta2 = Mfl_ArcTan2_f32(sin_theta2, cos_theta2) * RAD_TO_DEG_F32;
    
    /* Calculate shoulder angle (theta1) */
    k1 = l1 + l2 * cos_theta2;
    k2 = l2 * sin_theta2;
    
    if (Mfl_Abs_f32(k1) < 0.001f && Mfl_Abs_f32(k2) < 0.001f)
    {
        return 4u;  /* Singular configuration */
    }
    
    theta1 = Mfl_ArcTan2_f32(z_adj, y_adj) * RAD_TO_DEG_F32 - 
             Mfl_ArcTan2_f32(k2, k1) * RAD_TO_DEG_F32;
    
    /* Calculate wrist angle (theta3) to maintain end effector orientation */
    theta3 = Alpha_f32 - theta1 - theta2;
    
    /* Normalize angles to [-180, 180] range */
    theta0 = Rob_LimitAngle(theta0, -180.0f, 180.0f);
    theta1 = Rob_LimitAngle(theta1, -90.0f, 90.0f);
    theta2 = Rob_LimitAngle(theta2, -135.0f, 135.0f);
    theta3 = Rob_LimitAngle(theta3, -90.0f, 90.0f);
    
    /* Check joint limits */
    if (theta1 < -90.0f || theta1 > 90.0f || 
        theta2 < -135.0f || theta2 > 135.0f ||
        theta3 < -90.0f || theta3 > 90.0f)
    {
        return 5u;  /* Joint limits exceeded */
    }
    
    /* Store calculated angles */
    State_pst->TargetAngles.Theta[0] = theta0;
    State_pst->TargetAngles.Theta[1] = theta1;
    State_pst->TargetAngles.Theta[2] = theta2;
    State_pst->TargetAngles.Theta[3] = theta3;
    State_pst->TargetAngles.Theta[4] = 0.0f;  /* Wrist roll */
    State_pst->TargetAngles.Theta[5] = 0.0f;  /* Gripper */
    
    /* Update target position */
    State_pst->TargetPos.X = X_f32;
    State_pst->TargetPos.Y = Y_f32;
    State_pst->TargetPos.Z = Z_f32;
    State_pst->TargetPos.Alpha = Alpha_f32;
    
    return 0u;  /* Success */
}

/* Forward kinematics calculation using EASML trigonometric functions */
void Rob_ForwardKinematics(const Rob_JointAngles_Type *Angles_cpst,
                          const Rob_LinkConfig_Type *Config_cpst,
                          Rob_CartesianPos_Type *Position_pst)
{
    float32 theta0, theta1, theta2, theta3;
    float32 l0, l1, l2, l3;
    float32 theta0_rad, theta1_rad, theta2_rad, theta3_rad;
    float32 cos0, sin0, cos1, sin1, cos2, sin2, cos12, sin12;
    float32 r, z;
    
    /* Get angles and convert to radians */
    theta0 = Angles_cpst->Theta[0];
    theta1 = Angles_cpst->Theta[1];
    theta2 = Angles_cpst->Theta[2];
    theta3 = Angles_cpst->Theta[3];
    
    theta0_rad = theta0 * DEG_TO_RAD_F32;
    theta1_rad = theta1 * DEG_TO_RAD_F32;
    theta2_rad = theta2 * DEG_TO_RAD_F32;
    theta3_rad = theta3 * DEG_TO_RAD_F32;
    
    /* Get link lengths */
    l0 = Config_cpst->L0;
    l1 = Config_cpst->L1;
    l2 = Config_cpst->L2;
    l3 = Config_cpst->L3;
    
    /* Calculate trigonometric functions using EASML */
    cos0 = Mfl_Cos_f32(theta0_rad);
    sin0 = Mfl_Sin_f32(theta0_rad);
    cos1 = Mfl_Cos_f32(theta1_rad);
    sin1 = Mfl_Sin_f32(theta1_rad);
    cos2 = Mfl_Cos_f32(theta2_rad);
    sin2 = Mfl_Sin_f32(theta2_rad);
    
    cos12 = Mfl_Cos_f32(theta1_rad + theta2_rad);
    sin12 = Mfl_Sin_f32(theta1_rad + theta2_rad);
    
    /* Calculate wrist center position */
    r = l1 * cos1 + l2 * cos12;
    z = l0 + l1 * sin1 + l2 * sin12;
    
    /* Add end effector offset */
    float32 alpha_rad = (theta1 + theta2 + theta3) * DEG_TO_RAD_F32;
    r += l3 * Mfl_Cos_f32(alpha_rad);
    z += l3 * Mfl_Sin_f32(alpha_rad);
    
    /* Convert to Cartesian coordinates */
    Position_pst->X = r * sin0;
    Position_pst->Y = r * cos0;
    Position_pst->Z = z;
    Position_pst->Alpha = theta1 + theta2 + theta3;
    Position_pst->Beta = 0.0f;  /* No roll in this configuration */
}

/* Set servo angle with time constraint */
void Rob_SetServoAngle(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8, 
                       float32 Angle_f32, uint16 Time_u16)
{
    if (ServoIndex_u8 < ROB_SERVO_NUM)
    {
        State_pst->ServoControl.TargetAngle[ServoIndex_u8] = Angle_f32;
        State_pst->ServoControl.MoveTime[ServoIndex_u8] = Time_u16;
        State_pst->ServoControl.IsMoving[ServoIndex_u8] = TRUE;
        
        /* Convert angle to PWM using linear interpolation */
        State_pst->ServoControl.PwmValue[ServoIndex_u8] = Rob_AngleToPwm(Angle_f32, ServoIndex_u8);
    }
}

/* Set all servo angles simultaneously */
void Rob_SetAllServoAngles(Rob_SystemState_Type *State_pst, 
                          const float32 Angles_pf32[ROB_SERVO_NUM], uint16 Time_u16)
{
    uint8 i;
    
    for (i = 0u; i < ROB_SERVO_NUM; i++)
    {
        Rob_SetServoAngle(State_pst, i, Angles_pf32[i], Time_u16);
    }
}

/* Convert angle to PWM value */
uint16 Rob_AngleToPwm(float32 Angle_f32, uint8 ServoIndex_u8)
{
    float32 pwm_f32;
    uint16 pwm_u16;
    
    /* Linear mapping from angle (-135° to +135°) to PWM (500μs to 2500μs) */
    /* Using EASML limiting function */
    Angle_f32 = Mfl_Limit_f32(Angle_f32, -135.0f, 135.0f);
    
    /* Calculate PWM value */
    pwm_f32 = ROB_PWM_CENTER + (Angle_f32 * (ROB_PWM_MAX - ROB_PWM_MIN)) / 270.0f;
    
    /* Convert to integer and limit */
    pwm_u16 = (uint16)Mfl_Round_f32(pwm_f32);
    pwm_u16 = (pwm_u16 < ROB_PWM_MIN) ? ROB_PWM_MIN : pwm_u16;
    pwm_u16 = (pwm_u16 > ROB_PWM_MAX) ? ROB_PWM_MAX : pwm_u16;
    
    return pwm_u16;
}

/* Convert PWM value to angle */
float32 Rob_PwmToAngle(uint16 PwmValue_u16, uint8 ServoIndex_u8)
{
    float32 angle_f32;
    
    /* Limit PWM value */
    PwmValue_u16 = (PwmValue_u16 < ROB_PWM_MIN) ? ROB_PWM_MIN : PwmValue_u16;
    PwmValue_u16 = (PwmValue_u16 > ROB_PWM_MAX) ? ROB_PWM_MAX : PwmValue_u16;
    
    /* Convert to angle */
    angle_f32 = ((float32)(sint16)(PwmValue_u16 - ROB_PWM_CENTER)) * 270.0f / 
                (float32)(ROB_PWM_MAX - ROB_PWM_MIN);
    
    return angle_f32;
}

/* Move to Cartesian position */
void Rob_MoveToPosition(Rob_SystemState_Type *State_pst, 
                       float32 X_f32, float32 Y_f32, float32 Z_f32, 
                       float32 Alpha_f32, uint16 Time_u16)
{
    uint8 result;
    uint8 i;
    
    /* Calculate inverse kinematics */
    result = Rob_InverseKinematics(X_f32, Y_f32, Z_f32, Alpha_f32, State_pst);
    
    if (result == 0u)
    {
        /* Set all servos to calculated angles */
        for (i = 0u; i < ROB_DOF; i++)
        {
            Rob_SetServoAngle(State_pst, i, State_pst->TargetAngles.Theta[i], Time_u16);
        }
    }
}

/* Check if movement is complete */
boolean Rob_IsMovementComplete(const Rob_SystemState_Type *State_cpst)
{
    uint8 i;
    
    for (i = 0u; i < ROB_SERVO_NUM; i++)
    {
        if (State_cpst->ServoControl.IsMoving[i] == TRUE)
        {
            return FALSE;
        }
    }
    
    return TRUE;
}

/* Initialize PID controller for servo */
void Rob_InitPID(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8,
                float32 Kp_f32, float32 Ki_f32, float32 Kd_f32)
{
    if (ServoIndex_u8 < ROB_SERVO_NUM)
    {
        /* Initialize PID using EASML functions */
        Mfl_PIDSetParam(&State_pst->PidParams[ServoIndex_u8], Kp_f32, Kd_f32, 1.0f / Ki_f32);
        Mfl_PIDSetState(&State_pst->PidStates[ServoIndex_u8], 0.0f, 0.0f, 0.0f);
    }
}

/* Update PID controller */
void Rob_UpdatePID(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8, 
                  float32 Setpoint_f32, float32 Feedback_f32, float32 dT_f32)
{
    float32 error_f32;
    float32 output_f32;
    
    if (ServoIndex_u8 < ROB_SERVO_NUM)
    {
        error_f32 = Setpoint_f32 - Feedback_f32;
        
        /* Calculate PID output using EASML */
        Mfl_PIDTyp1Calc(error_f32, &State_pst->PidStates[ServoIndex_u8],
                       &State_pst->PidParams[ServoIndex_u8], dT_f32);
        
        output_f32 = Mfl_PIDOut_f32(&State_pst->PidStates[ServoIndex_u8]);
        
        /* Apply output to servo */
        State_pst->ServoControl.CurrentAngle[ServoIndex_u8] += output_f32;
        State_pst->ServoControl.PwmValue[ServoIndex_u8] = 
            Rob_AngleToPwm(State_pst->ServoControl.CurrentAngle[ServoIndex_u8], ServoIndex_u8);
    }
}

/* Utility function: Convert degrees to radians */
float32 Rob_DegToRad(float32 Degrees_f32)
{
    return Degrees_f32 * DEG_TO_RAD_F32;
}

/* Utility function: Convert radians to degrees */
float32 Rob_RadToDeg(float32 Radians_f32)
{
    return Radians_f32 * RAD_TO_DEG_F32;
}

/* Utility function: Limit angle to specified range */
float32 Rob_LimitAngle(float32 Angle_f32, float32 Min_f32, float32 Max_f32)
{
    return Mfl_Limit_f32(Angle_f32, Min_f32, Max_f32);
}
